#include "mathlib.h"
#include <glm\gtc\matrix_transform.hpp>
#include <glm\gtc\type_ptr.hpp>

namespace HW
{

double Pi=4*atan(1.0);

int sgn(flt num)
{
	if (num > 0) return(1);
	else if (num < 0) return(-1);
	else return(0);
}

flt Curve(flt newvalue, flt oldvalue, flt increments)
{
	int sign;
	flt slip;
	sign=sgn(oldvalue-newvalue);
	slip=(oldvalue-newvalue)/increments;
	oldvalue-=slip;
	if(sign!=sgn(oldvalue-newvalue)) return(newvalue);
	return(oldvalue);
}


float Clamp(float value, float min, float max)
{
	if(value<min) value=min;
	else if(value>max) value=max;
	return value;
}


float PlaneDistanceToPoint(const TVec4& plane, const TVec3& point)
{
	return plane.X*point.X+plane.Y*point.Y+plane.Z*point.Z+plane.W;
}

int Min(int a, int b)
{
	return (a<b)?a:b;
}

float Min(float a, float b)
{
	return (a<b)?a:b;
}

double Min(double a, double b)
{
	return (a<b)?a:b;
}

int Max(int a, int b)
{
	return (a>b)?a:b;
}

float Max(float a, float b)
{
	return (a>b)?a:b;
}

double Max(double a, double b)
{
	return (a>b)?a:b;
}

double Log2(double n)
{
	return log(n)/log(2.0);
}

double deg2rad(double d)
{
	return (Pi/180)*d;				// twice faster than: return (d*Pi)/180.0;
}

double rad2deg(double r)
{
	return (180/Pi)*r;				// twice faster than: return (r*180.0)/Pi;
}

double RoundToInt(double num)
{
	return (num < 0.0) ? ceil(num - 0.5) : floor(num + 0.5);
}

double Round(double num, int places)
{
	double off = pow( 10.0, places );
	return RoundToInt( num * off ) / off;
}

const TVec2 operator+(const TVec2& lhs, const TVec2& rhs)
{
	TVec2 ret = lhs;
	ret += rhs;
	return ret;
}

const TVec2 operator+(const TVec2& lhs, const flt rhs)
{
	TVec2 ret = lhs;
	ret += rhs;
	return ret;
}

const TVec3 operator+(const TVec3& lhs, const TVec3& rhs)
{
	TVec3 ret = lhs;
	ret += rhs;
	return ret;
}

const TVec3 operator+(const TVec3& lhs, const flt rhs)
{
	TVec3 ret = lhs;
	ret += rhs;
	return ret;
}

const TVec4 operator+(const TVec4& lhs, const TVec4& rhs)
{
	TVec4 ret = lhs;
	ret += rhs;
	return ret;
}

const TVec4 operator+(const TVec4& lhs, const flt rhs)
{
	TVec4 ret = lhs;
	ret += rhs;
	return ret;
}

const TVec2& operator+=(TVec2& lhs, const TVec2& rhs)
{
	lhs.X+=rhs.X;
	lhs.Y+=rhs.Y;
	return lhs;
}

const TVec2& operator+=(TVec2& lhs, const flt rhs)
{
	lhs.X+=rhs;
	lhs.Y+=rhs;
	return lhs;
}

const TVec3& operator+=(TVec3& lhs, const TVec3& rhs)
{
	lhs.X+=rhs.X;
	lhs.Y+=rhs.Y;
	lhs.Z+=rhs.Z;
	return lhs;
}

const TVec3& operator+=(TVec3& lhs, const flt rhs)
{
	lhs.X+=rhs;
	lhs.Y+=rhs;
	lhs.Z+=rhs;
	return lhs;
}

const TVec4& operator+=(TVec4& lhs, const TVec4& rhs)
{
	lhs.X+=rhs.X;
	lhs.Y+=rhs.Y;
	lhs.Z+=rhs.Z;
	lhs.W+=rhs.W;
	return lhs;
}

const TVec4& operator+=(TVec4& lhs, const flt rhs)
{
	lhs.X+=rhs;
	lhs.Y+=rhs;
	lhs.Z+=rhs;
	lhs.W+=rhs;
	return lhs;
}

const TVec2 operator-(const TVec2& lhs, const TVec2& rhs)
{
	TVec2 ret = lhs;
	ret -= rhs;
	return ret;
}

const TVec2 operator-(const TVec2& lhs, const flt rhs)
{
	TVec2 ret = lhs;
	ret -= rhs;
	return ret;
}

const TVec3 operator-(const TVec3& lhs, const TVec3& rhs)
{
	TVec3 ret = lhs;
	ret -= rhs;
	return ret;
}

const TVec3 operator-(const TVec3& lhs, const flt rhs)
{
	TVec3 ret = lhs;
	ret -= rhs;
	return ret;
}

const TVec4 operator-(const TVec4& lhs, const TVec4& rhs)
{
	TVec4 ret = lhs;
	ret -= rhs;
	return ret;
}

const TVec4 operator-(const TVec4& lhs, const flt rhs)
{
	TVec4 ret = lhs;
	ret -= rhs;
	return ret;
}

const TVec2& operator-=(TVec2& lhs, const TVec2& rhs)
{
	lhs.X-=rhs.X;
	lhs.Y-=rhs.Y;
	return lhs;
}

const TVec2& operator-=(TVec2& lhs, const flt rhs)
{
	lhs.X-=rhs;
	lhs.Y-=rhs;
	return lhs;
}

const TVec3& operator-=(TVec3& lhs, const TVec3& rhs)
{
	lhs.X-=rhs.X;
	lhs.Y-=rhs.Y;
	lhs.Z-=rhs.Z;
	return lhs;
}

const TVec3& operator-=(TVec3& lhs, const flt rhs)
{
	lhs.X-=rhs;
	lhs.Y-=rhs;
	lhs.Z-=rhs;
	return lhs;
}

const TVec4& operator-=(TVec4& lhs, const TVec4& rhs)
{
	lhs.X-=rhs.X;
	lhs.Y-=rhs.Y;
	lhs.Z-=rhs.Z;
	lhs.W-=rhs.W;
	return lhs;
}

const TVec4& operator-=(TVec4& lhs, const flt rhs)
{
	lhs.X-=rhs;
	lhs.Y-=rhs;
	lhs.Z-=rhs;
	lhs.W-=rhs;
	return lhs;
}

const TVec2 operator*(const TVec2& lhs, const TVec2& rhs)
{
	TVec2 ret = lhs;
	ret *= rhs;
	return ret;
}

const TVec2 operator*(const TVec2& lhs, const flt rhs)
{
	TVec2 ret = lhs;
	ret *= rhs;
	return ret;
}

const TVec3 operator*(const TVec3& lhs, const TVec3& rhs)
{
	TVec3 ret = lhs;
	ret *= rhs;
	return ret;
}

const TVec3 operator*(const TVec3& lhs, const flt rhs)
{
	TVec3 ret = lhs;
	ret *= rhs;
	return ret;
}

const TVec4 operator*(const TVec4& lhs, const TVec4& rhs)
{
	TVec4 ret = lhs;
	ret *= rhs;
	return ret;
}

const TVec4 operator*(const TVec4& lhs, const flt rhs)
{
	TVec4 ret = lhs;
	ret *= rhs;
	return ret;
}

const TVec2& operator*=(TVec2& lhs, const TVec2& rhs)
{
	lhs.X*=rhs.X;
	lhs.Y*=rhs.Y;
	return lhs;
}

const TVec2& operator*=(TVec2& lhs, const flt rhs)
{
	lhs.X*=rhs;
	lhs.Y*=rhs;
	return lhs;
}

const TVec3& operator*=(TVec3& lhs, const TVec3& rhs)
{
	lhs.X*=rhs.X;
	lhs.Y*=rhs.Y;
	lhs.Z*=rhs.Z;
	return lhs;
}

const TVec3& operator*=(TVec3& lhs, const flt rhs)
{
	lhs.X*=rhs;
	lhs.Y*=rhs;
	lhs.Z*=rhs;
	return lhs;
}

const TVec4& operator*=(TVec4& lhs, const TVec4& rhs)
{
	lhs.X*=rhs.X;
	lhs.Y*=rhs.Y;
	lhs.Z*=rhs.Z;
	lhs.W*=rhs.W;
	return lhs;
}

const TVec4& operator*=(TVec4& lhs, const flt rhs)
{
	lhs.X*=rhs;
	lhs.Y*=rhs;
	lhs.Z*=rhs;
	lhs.W*=rhs;
	return lhs;
}

const TVec2 operator/(const TVec2& lhs, const TVec2& rhs)
{
	TVec2 ret = lhs;
	ret /= rhs;
	return ret;
}

const TVec2 operator/(const TVec2& lhs, const flt rhs)
{
	TVec2 ret = lhs;
	ret /= rhs;
	return ret;
}

const TVec3 operator/(const TVec3& lhs, const TVec3& rhs)
{
	TVec3 ret = lhs;
	ret /= rhs;
	return ret;
}

const TVec3 operator/(const TVec3& lhs, const flt rhs)
{
	TVec3 ret = lhs;
	ret /= rhs;
	return ret;
}

const TVec4 operator/(const TVec4& lhs, const TVec4& rhs)
{
	TVec4 ret = lhs;
	ret /= rhs;
	return ret;
}

const TVec4 operator/(const TVec4& lhs, const flt rhs)
{
	TVec4 ret = lhs;
	ret /= rhs;
	return ret;
}

const TVec2& operator/=(TVec2& lhs, const TVec2& rhs)
{
	lhs.X/=rhs.X;
	lhs.Y/=rhs.Y;
	return lhs;
}

const TVec2& operator/=(TVec2& lhs, const flt rhs)
{
	lhs.X/=rhs;
	lhs.Y/=rhs;
	return lhs;
}

const TVec3& operator/=(TVec3& lhs, const TVec3& rhs)
{
	lhs.X/=rhs.X;
	lhs.Y/=rhs.Y;
	lhs.Z/=rhs.Z;
	return lhs;
}

const TVec3& operator/=(TVec3& lhs, const flt rhs)
{
	lhs.X/=rhs;
	lhs.Y/=rhs;
	lhs.Z/=rhs;
	return lhs;
}

const TVec4& operator/=(TVec4& lhs, const TVec4& rhs)
{
	lhs.X/=rhs.X;
	lhs.Y/=rhs.Y;
	lhs.Z/=rhs.Z;
	lhs.W/=rhs.W;
	return lhs;
}

const TVec4& operator/=(TVec4& lhs, const flt rhs)
{
	lhs.X/=rhs;
	lhs.Y/=rhs;
	lhs.Z/=rhs;
	lhs.W/=rhs;
	return lhs;
}

const bool operator==(const TVec2& lhs, const TVec2& rhs)
{
	return (lhs.X==rhs.X) && (lhs.Y==rhs.Y);
}

const bool operator==(const TVec2& lhs, const flt rhs)
{
	return (lhs.X==rhs) && (lhs.Y==rhs);
}

const bool operator==(const TVec3& lhs, const TVec3& rhs)
{
	return (lhs.X==rhs.X) && (lhs.Y==rhs.Y) && (lhs.Z==rhs.Z);
}

const bool operator==(const TVec3& lhs, const flt rhs)
{
	return (lhs.X==rhs) && (lhs.Y==rhs) && (lhs.Z==rhs);
}

const bool operator==(const TVec4& lhs, const TVec4& rhs)
{
	return (lhs.X==rhs.X) && (lhs.Y==rhs.Y) && (lhs.Z==rhs.Z) && (lhs.W==rhs.W);
}

const bool operator==(const TVec4& lhs, const flt rhs)
{
	return (lhs.X==rhs) && (lhs.Y==rhs) && (lhs.Z==rhs) && (lhs.W==rhs);
}

const bool operator!=(const TVec2& lhs, const TVec2& rhs)
{
	return (lhs.X!=rhs.X) || (lhs.Y!=rhs.Y);
}

const bool operator!=(const TVec2& lhs, const flt rhs)
{
	return (lhs.X!=rhs) || (lhs.Y!=rhs);
}

const bool operator!=(const TVec3& lhs, const TVec3& rhs)
{
	return (lhs.X!=rhs.X) || (lhs.Y!=rhs.Y) || (lhs.Z!=rhs.Z);
}

const bool operator!=(const TVec3& lhs, const flt rhs)
{
	return (lhs.X!=rhs) || (lhs.Y!=rhs) || (lhs.Z!=rhs);
}

const bool operator!=(const TVec4& lhs, const TVec4& rhs)
{
	return (lhs.X!=rhs.X) || (lhs.Y!=rhs.Y) || (lhs.Z!=rhs.Z) || (lhs.W!=rhs.W);
}

const bool operator!=(const TVec4& lhs, const flt rhs)
{
	return (lhs.X!=rhs) || (lhs.Y!=rhs) || (lhs.Z!=rhs) || (lhs.W!=rhs);
}

const TVec4 operator*(const TVec16& lhs, const TVec4& rhs)
{
	TVec4 ans;
	memcpy ( ans.a , glm::value_ptr(glm::make_mat4x4(lhs.a)*glm::make_vec4(rhs.a)) , 16*sizeof(flt) );
	return ans;
}

double Length(const TVec2& v)
{
	return sqrt(v.X*v.X+v.Y*v.Y);
}

double Length(const TVec3& v)
{
	return sqrt(v.X*v.X+v.Y*v.Y+v.Z*v.Z);
}

double Length(const TVec4& v)
{
	return sqrt(v.X*v.X+v.Y*v.Y+v.Z*v.Z+v.W*v.W);
}

const TVec2 Normalize(const TVec2& v)
{
	TVec2 ret = v;
	ret /= Vec2((flt)Length(v));
	return ret;
}

const TVec3 Normalize(const TVec3& v)
{
	TVec3 ret = v;
	ret /= Vec3((flt)Length(v));
	return ret;
}

const TVec4 Normalize(const TVec4& v)
{
	TVec4 ret = v;
	ret /= Vec4((flt)Length(v));
	return ret;
}

double WrapAngle(double curangle)
{
	return curangle-floor(curangle/360.0)*360.0;
}

double CurveAngle(double destinationAngle, double currentAngle, double steps)
{
	double diff = WrapAngle(WrapAngle(currentAngle)-WrapAngle(destinationAngle));
	if(diff<180)
	{
		diff = -diff;
	}
	else
	{
		diff = 360 - diff;
	}
	double newValue = WrapAngle(currentAngle + diff/steps);
	return newValue;
}

double Lerp(double value1, double value2, double amount)
{
	return value1+amount*(value2-value1);
}

TVec4 Slerp(TVec4 qa, TVec4 qb, double t)
{
   TVec4 qm;
   double cosHalfTheta = qa.W * qb.W + qa.X * qb.X + qa.Y * qb.Y + qa.Z * qb.Z;

   if (fabs(cosHalfTheta) >= 1.0)
   {
      qm.W = qa.W;qm.X = qa.X;qm.Y = qa.Y;qm.Z = qa.Z;
      return qm;
   }

   double halfTheta = acos(cosHalfTheta);
   double sinHalfTheta = sqrt(1.0 - cosHalfTheta*cosHalfTheta);

   if (fabs(sinHalfTheta) < 0.001)
   {
      qm.W = (flt)(qa.W * 0.5 + qb.W * 0.5);
      qm.X = (flt)(qa.X * 0.5 + qb.X * 0.5);
      qm.Y = (flt)(qa.Y * 0.5 + qb.Y * 0.5);
      qm.Z = (flt)(qa.Z * 0.5 + qb.Z * 0.5);
      return qm;
   }
   
   double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
   double ratioB = sin(t * halfTheta) / sinHalfTheta;

   qm.W = (flt)(qa.W * ratioA + qb.W * ratioB);
   qm.X = (flt)(qa.X * ratioA + qb.X * ratioB);
   qm.Y = (flt)(qa.Y * ratioA + qb.Y * ratioB);
   qm.Z = (flt)(qa.Z * ratioA + qb.Z * ratioB);
   return qm;
}

TVec3 QuatToEuler(TVec4 quat)
{
	double x2=quat.X*quat.X;
	double y2=quat.Y*quat.Y;
	double z2=quat.Z*quat.Z;

	double rotyrad = atan2((double)(2 * quat.Y * quat.W -2 * quat.Z * quat.X), (1-2*y2-2*x2) );
	double rotxrad =-asin (Clamp   (2 * quat.Z * quat.Y +2 * quat.X * quat.W , -1.0,1.0)     );
	double rotzrad =-atan2((double)(2 * quat.Z * quat.W -2 * quat.Y * quat.X), (1-2*z2-2*x2) );

	TVec3 rot;
	rot.X = (flt)rad2deg(rotxrad);
	rot.Y = (flt)rad2deg(rotyrad);
	rot.Z = (flt)rad2deg(rotzrad);
	return rot;
}

double Modulate(double a, double b)
{
	a=fmod(a,b);
	if(a<0)a=b+a;
	return a;
}

double random(double min, double max)
{
	double interval = max - min;
	return min + interval * rand()/(double)RAND_MAX;
}

double rnd(void)
{
	return (double)rand()/RAND_MAX;
}

void Randomize(void)
{
	UpdateAppTime();
	srand((int)AppTime());
}
};